#pragma once
#include <fstream>
#include <vector>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

#include <iostream>
#include <opencv2/opencv.hpp>
#include <eigen3/Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/affine.hpp>

#include <chrono>
using namespace std;
using namespace cv;
#include <sys/time.h>

static bool Mat_read_binary(cv::Mat &img_vec, string filename) // 整体读出
{
    int channl(0);
    int rows(0);
    int cols(0);
    short type(0);
    short em_size(0);
    ifstream fin(filename, ios::binary);
    fin.read((char *)&channl, 1);
    fin.read((char *)&type, 1);
    fin.read((char *)&em_size, 2);
    fin.read((char *)&cols, 4);
    fin.read((char *)&rows, 4);
    printf("SAVE:cols=%d,type=%d,em_size=%d,rows=%d,channels=%d\n", cols, type, em_size, rows, channl);
    img_vec = cv::Mat(rows, cols, type);
    fin.read((char *)&img_vec.data[0], rows * cols * em_size);
    fin.close();
    return true;
}

static __time_t GetUTC() //   微秒
{
    struct timeval tv;
    gettimeofday(&tv, NULL);
    // stringstream s;
    // s<<tv.tv_sec;
    // printf("second:%ld \n", tv.tv_sec);                                 //秒
    // printf("millisecond:%ld \n", tv.tv_sec * 1000 + tv.tv_usec / 1000); //毫秒
    // printf("microsecond:%ld \n", tv.tv_sec * 1000000 + tv.tv_usec);     //微秒
    return tv.tv_sec * 1000000 + tv.tv_usec;
}
template <class _Tp>
class Pose
{
public:
    vector<Eigen::Quaternion<_Tp>> qs;
    vector<Eigen::Matrix<_Tp, 3, 1>> ts;
    vector<string> depths;
    vector<string> rgbs;
    vector<uint64_t> times;
    uint16_t frames = 0;

    Eigen::Matrix<_Tp, 4, 4> operator()(int i)
    {
        Eigen::Matrix<_Tp, 4, 4> T2 = Eigen::Matrix<_Tp, 4, 4>::Identity();
        // Eigen::Matrix<_Tp,4,4> T2;
        // T2.setIdentity();
        // Eigen::Matrix4d T2;
        //  T2.block<3,3>(0, 0)
        //    t1 =
        T2.topLeftCorner(3, 3) = qs[i].toRotationMatrix();
        T2.topRightCorner(3, 1) = ts[i];

        // Eigen::Isometry3f T;
        // T.setIdentity();
        // T.pretranslate(ts[id]).rotate(qs[id]);
        return T2;
    }
    cv::Affine3f getvectorPose(int i)
    {
        cv::Vec<float, 16> m_pose;
        auto mp = (*this)(i);
        cv::Mat rere;
        cv::eigen2cv(mp, rere);
        m_pose = rere.at<cv::Vec<float, 16>>(0, 0); // poses.at<cv::Vec<float, 16>>(ind, 0);
        return cv::Affine3f(m_pose.val);
    }
    void getvectorPose(int i, cv::Vec<float, 16> &m_pose)
    {

        auto mp = (*this)(i);
        cv::Mat rere;
        cv::eigen2cv(mp, rere);
        m_pose = rere.at<cv::Vec<float, 16>>(0, 0); // poses.at<cv::Vec<float, 16>>(ind, 0);
        return;
    }
    void totxt()
    {
        cout << "#id  times x y z  (x y z w)" << endl;
        for (int i = 0; i < times.size(); i++)
        {
            // cout << times[i] <<ts[0]<<ts[1]<<ts[2]<< endl;
            printf("%d %ld %f %f %f ", i + 1, times[i], ts[i][0], ts[i][1], ts[i][2]);
            printf("%f %f %f %f\n", qs[i].x(), qs[i].y(), qs[i].z(), qs[i].w());
        }
    }
    void tofile()
    {
        fstream fs(path + "/ax.txt", ios::out);
        cout << path + "/ax.txt" << endl;
        fs << "#id  times x y z  (x y z w)" << endl;
        for (int i = 0; i < times.size(); i++)
        {
            // cout << times[i] <<ts[0]<<ts[1]<<ts[2]<< endl;
            fs << cv::format("%d %ld %f %f %f ", i + 1, times[i], ts[i][0], ts[i][1], ts[i][2]);
            fs << cv::format("%f %f %f %f\n", qs[i].x(), qs[i].y(), qs[i].z(), qs[i].w());
        }
        fs.close();
    }
    string path;
    void loadlivingRoom(string path)
    {
        this->path = path;
        fstream fs(path + "/livingRoom.gt.freiburg", ios::in);
        if (!fs.is_open())
        {
            // cout << str << endl;
            assert(0);
        }

        string temp;
        getline(fs, temp);
        uint64_t start = GetUTC();
        while (true)
        {
            _Tp idx;
            fs >> idx;

            // fs >> start;
            _Tp _pose[7];
            for (int i = 0; i < 7; i++)
                fs >> _pose[i];

            if (!fs.good())
                break;
            Eigen::Quaternion<_Tp> q(_pose[6], _pose[3], _pose[4], _pose[5]); // w xyz
            qs.push_back(q);
            ts.push_back(Eigen::Matrix<_Tp, 3, 1>(_pose[0], _pose[1], _pose[2]));
            start += 30000;
            times.push_back(start);
        }
        frames = times.size();

        // for (int i = 0; i < frames; i++)
        // {
        //     auto depth = cv::imread(cv::format("%s/depth/%d.png", path.c_str(), i), 2);
        //     cv::imwrite(cv::format("%s/depth/%d.pgm", path.c_str(), i), depth);
        //     auto rgb = cv::imread(cv::format("%s/rgb/%d.png", path.c_str(), i));
        //     cv::imwrite(cv::format("%s/rgb/%d.ppm", path.c_str(), i), rgb);
        // }
    }
    void load(string path)
    {
        this->path = path;
        fstream fs(path + "/ax.txt", ios::in);
        if (!fs.is_open())
        {
            cout << path + "/ax.txt" << endl;
            assert(0);
        }

        string temp;
        getline(fs, temp);
        uint64_t start = GetUTC();
        while (true)
        {
            _Tp idx;
            fs >> idx;

            fs >> start;
            _Tp _pose[7];
            for (int i = 0; i < 7; i++)
                fs >> _pose[i];

            if (!fs.good())
                break;
            Eigen::Quaternion<_Tp> q(_pose[6], _pose[3], _pose[4], _pose[5]); // w xyz
            qs.push_back(q);
            ts.push_back(Eigen::Matrix<_Tp, 3, 1>(_pose[0], _pose[1], _pose[2]));
            times.push_back(start);
        }
        frames = times.size();

        // for (int i = 0; i < frames; i++)
        // {
        //     auto depth = cv::imread(cv::format("%s/depth/%d.png", path.c_str(), i), 2);
        //     cv::imwrite(cv::format("%s/depth/%d.pgm", path.c_str(), i), depth);
        //     auto rgb = cv::imread(cv::format("%s/rgb/%d.png", path.c_str(), i));
        //     cv::imwrite(cv::format("%s/rgb/%d.ppm", path.c_str(), i), rgb);
        // }
    }
    void loadtum(string path)
    {
        this->path = path;

        fstream fs("./a2.txt", ios::in);
        if (!fs.is_open())
        {
            cout << "./a2.txt" << endl;
            assert(0);
        }

        string temp;

        while (true)
        {
            double idx;
            fs >> idx;
            uint64_t start = idx * 1000000;
            // fs >> start;
            _Tp _pose[7];
            for (int i = 0; i < 7; i++)
                fs >> _pose[i];

            fs >> temp;
            depths.push_back(temp);
            auto depth = cv::imread(path + "/" + temp, 2);
            // Mat depth5,rgb5;
            // depth.convertTo(depth5,-1, 0.2, 0);
            cv::imwrite(cv::format("%s/depth/%ld.pgm", path.c_str(), qs.size()), depth);
            fs >> temp;
            auto rgb = cv::imread(path + "/" + temp);
            cv::imwrite(cv::format("%s/rgb/%ld.ppm", path.c_str(), qs.size()), rgb);
            rgbs.push_back(temp);
            if (!fs.good())
                break;
            Eigen::Quaternion<_Tp> q(_pose[6], _pose[3], _pose[4], _pose[5]); // w xyz
            qs.push_back(q);
            ts.push_back(Eigen::Matrix<_Tp, 3, 1>(_pose[0], _pose[1], _pose[2]));
            times.push_back(start);
        }
        frames = times.size();
    }
    void loadNERF(string path)
    {
        this->path = path;
        this->path = path;
        fstream fs(path + "/poses.txt", ios::in); // trainval_
        if (!fs.is_open())
        {
            cout << path << endl;
            assert(0);
        }

        int idx = 0;
        // _Tp _pose2[16];
        // for (int i = 0; i < 16; i++)
        //     fs >> _pose2[i];
        uint64_t start_time = GetUTC();
        cv::Point3f start;
        while (true)
        {
            _Tp _pose[16];
            _Tp *p = new _Tp[16];
            _Tp *p2 = new _Tp[16];
            for (int i = 0; i < 16; i++)
            {
                fs >> p[i];
                p2[i] = p[i];
            }
            // p[0] = p2[0], p[1] = -p2[1], p[2] = -p2[2];
            // p[4] = -p2[8], p[5] = p2[9], p[6] = p2[10];
            // p[8] = p2[4], p[9] = -p2[5], p[10] = -p2[6];

            // p[7] = -p2[11];
            // p[11] = p2[7];

            p[0] = p2[0], p[1] = -p2[1], p[2] = -p2[2];
            p[8] = p2[8], p[9] = -p2[9], p[10] = -p2[10];
            p[4] = p2[4], p[5] = -p2[5], p[6] = -p2[6];
            p[11] = p2[11];
            p[7] = p2[7];
            // // swap(p[1], p[4]);
            // // swap(p[2], p[8]);
            // // swap(p[9], p[6]);
            // // swap(p[1], [4]);
            // // p[4] = p2[8], p[5] = p2[9], p[6] = p2[10];
            // // p[8] = p2[4], p[9] = p2[5], p[10] = p2[6];

            // if (idx == 0)
            // {
            //     start.x = p[3];
            //     start.y = p[7];
            //     start.z = p[11];
            //     p[3] = 0, p[7] = 0, p[11] = 0;
            // }
            // else
            // {
            //     p[3] -= start.x;
            //     p[7] -= start.y;
            //     p[11] -= start.z;
            // }
            if (!fs.good())
                break;
            cv::Affine3f af(p);
            cout << af.matrix << endl;

            // auto rgb = cv::imread(path + cv::format("/images/img%d.png", idx));
            // auto depth = cv::imread(path + cv::format("/depth/depth%d.png", idx), 2);
            // // cv::imshow("rgb", rgb);
            // // cv::imshow("depth", depth);
            // // cv::waitKey(10);
            // cv::imwrite(cv::format("%s/depth/%ld.png", path.c_str(), qs.size()), depth);
            // cv::imwrite(cv::format("%s/rgb/%ld.png", path.c_str(), qs.size()), rgb);

            auto _t = af.translation();
            cv::Affine3<float>::Mat3 rere = af.rotation();
            Eigen::Matrix3f mp;
            cv::cv2eigen(rere, mp);
            // cout << mp.matrix() << endl;
            Eigen::Quaternion<_Tp> q(mp); //_pose[6], _pose[3], _pose[4], _pose[5]); // w xyz
            qs.push_back(q);
            ts.push_back(Eigen::Matrix<_Tp, 3, 1>(_t[0], _t[1], _t[2]));
            uint64_t start_time = idx * 1000000;
            times.push_back(start_time);
            idx++;
            // break;
        }
    }
    void loadklg()
    {
        this->path = "/dataset/img";
        cv::Mat img_vec;
        Mat_read_binary(img_vec, "/dataset/img/matpose.bin");
        uint64_t start = GetUTC();
        for (int i; i < img_vec.rows; i++)
        {
            start += 30000; //  * 1000000;

            auto depth = cv::imread(cv::format("/dataset/img/depth/%04d.png", i), 2);
            // Mat depth5,rgb5;
            // depth.convertTo(depth5,-1, 0.2, 0);
            cv::imwrite(cv::format("%s/depth/%ld.pgm", path.c_str(), qs.size()), depth);
            auto temp = cv::format("rgb/%04d.png", i);
            auto rgb = cv::imread(path + "/" + temp);
            cv::imwrite(cv::format("%s/rgb/%ld.ppm", path.c_str(), qs.size()), rgb);
            rgbs.push_back(temp);
            auto &_pp = img_vec.at<cv::Vec<float, 16>>(i, 0);
            cv::Affine3f af(_pp.val); // poses.at<cv::Vec<float, 16>>(ind, 0);
            auto _t = af.translation();
            cv::Affine3<float>::Mat3 rere = af.rotation();
            Eigen::Matrix3f mp;
            cv::cv2eigen(rere, mp);
            // cout << mp.matrix() << endl;
            Eigen::Quaternion<_Tp> q(mp); //_pose[6], _pose[3], _pose[4], _pose[5]); // w xyz
            qs.push_back(q);
            ts.push_back(Eigen::Matrix<_Tp, 3, 1>(_t[0], _t[1], _t[2]));
            times.push_back(start);
        }
        frames = times.size();
    }
};

// /*void read_ic()
// {
//     fstream fs("/home/lei/dataset/paper/0/livingRoom.gt.freiburg", ios::in);

//     assert(fs.is_open());
//     Pose p;
//     uint64_t start = GetUTC();
//     while (true)
//     {
//         double idx;
//         fs >> idx;
//         double _pose[7];

//         for (int i = 0; i < 7; i++)
//         {
//             fs >> _pose[i];
//             // std::cout<<_pose[i]<<" ";
//         }

//         double marix[9];

//         Eigen::Quaterniond q(_pose[6], _pose[3], _pose[4], _pose[5]); // w xyz
//         p.qs.push_back(q);
//         p.ts.push_back(Eigen::Vector3d(_pose[0], _pose[1], _pose[2]));
//         p.times.push_back(start);
//         start += 30000;
//         if (!fs.good())
//             break;
//     }
// }*/
template <typename _Tp>
class DataSet
{
private:
    /* data */
public:
    Pose<_Tp> pose;
    Mat cam_K;
    vector<string> color_path;
    vector<string> depth_path;
    float depth_factor;
    int row = 480, col = 640;
    int endimage = 100;
    struct
    {
        float fx, fy, cx, cy;
    };
    DataSet(string datapath = "/home/u20/dataset/paper/e3", string yamlpath = "../info.yaml")
    {
        cv::FileStorage fs(datapath + "/info.yaml", cv::FileStorage::READ);
        if (!fs.isOpened())
        {
            cout << " 没有" << datapath + "/info.yaml" << endl;
            assert(0);
        }
        fs["cam_K"] >> cam_K;
        string posefile;
        fs["posefile"] >> posefile;
        pose.load(datapath);
        fs["posefile"] >> endimage;
        fs["depth_factor"] >> depth_factor;
        // fs["col"] >> col;
        // fs["row"] >> row;
        fs.release();
        cout << cam_K << endl;
        fx = cam_K.at<float>(0, 0);
        fy = cam_K.at<float>(1, 1);
        cx = cam_K.at<float>(0, 2);
        cy = cam_K.at<float>(1, 2);

        for (int i = 0; i < pose.frames; i++)
        {
            color_path.push_back(datapath + cv::format("/rgb/%d.png", i));
            depth_path.push_back(datapath + cv::format("/depth/%dd.png", i));
            // auto rgb = cv::imread(color_path.back());
            // cv::imwrite(datapath + cv::format("/rgb/%d.png",i) ,rgb);
            // auto depth = cv::imread(depth_path.back(), 2);

            // cv::imwrite(datapath + cv::format("/depth/%d.png",i), depth);
        }
        cv::Mat fimg = cv::imread(color_path[0]);
        col = fimg.cols;
        row = fimg.rows;
        printf("%d %d\n", col, row);
        // pose.totxt();
    }
    Mat rgb_, depth_src;
    cv::Vec<float, 16> m_pose;
    bool ReadNextTUM(int ind = 0)
    {
        // std::cout << end_frame_ << std::endl;
        if (ind < pose.frames)
        {
            rgb_ = cv::imread(color_path[ind + 1], cv::IMREAD_COLOR);
            if (rgb_.empty())
            {
                std::cout << color_path[ind + 1] << std::endl;
                return false;
            };
            // depth_src = cv::imread(mpath, cv::IMREAD_ANYDEPTH);
            // rgb_ = cv::imread(filebase_+"/"+color_paths[ind], cv::IMREAD_COLOR);
            // cv::imshow("rgb_1", rgb_);

            depth_src = cv::imread(depth_path[ind + 1], cv::IMREAD_ANYDEPTH);
            if (depth_src.empty())
            {
                std::cout << depth_path[ind + 1] << std::endl;
                return false;
            };
            // depth_src.convertTo(depth_, CV_32FC1, DEPTHFACTOR);
            cv::imshow("rgb_1", rgb_);

            auto mp = pose(ind);
            cv::Mat rere;
            cv::eigen2cv(mp, rere);
            cout << rere << endl;
            m_pose = rere.at<cv::Vec<float, 16>>(0, 0); // poses.at<cv::Vec<float, 16>>(ind, 0);
            // cout << mp.matrix() << " " << m_pose << endl;
            // cv::imshow("depth_1", rgb_);
            // cv::waitKey(1);
            // depth_src.convertTo(depth_, CV_32FC1, 1.0f / 5000);
            // index_++;
            // std::cout<<"L "<<"ind"<<end_frame_<<std::endl;

            return true;
        }
        else
        {
            std::cout << "cannot open file" << std::endl;
        }
        return false;
    }
};

// // int main()
// // {
// //     DataSet dt("/home/lei/dataset/paper/0");
// //     DataSet dt("/home/lei/dataset/paper/0");
// //     DataSet dt("/home/lei/dataset/paper/0");
